﻿using UnityEngine;
using System;
using System.Collections.Generic;
using System.Collections;
using Kinect2;

public class BodyBasic : MonoBehaviour {
	
	KinectSensor kinectSensor;
	BodyFrameReader bodyFrameReader;
    Body[] bodies;
	BodyObject[] bodyObjects = new BodyObject[Kinect.BODY_COUNT];
	CoordinateMapper coordinateMapper;
	GameObject status;
	GameObject left, right, top, bottom;

	void Start ()
    {
		kinectSensor = KinectSensor.GetDefault();
        coordinateMapper = kinectSensor.CoordinateMapper;

            
		bodyFrameReader = kinectSensor.BodyFrameSource.OpenReader();
        bodyFrameReader.FrameArrived += Reader_FrameArrived;

        kinectSensor.IsAvailableChanged += Sensor_IsAvailableChanged;
        kinectSensor.Open();

        InitializeComponent();
	}

	void Update () {
        EventRouter.Check();
	}

    void OnPostRender()
    {
        foreach (BodyObject bo in bodyObjects)
        {
            GL.Begin(GL.LINES);
            bo.DrawLines();
            GL.End();
        }
    }

    void OnApplicationQuit()
    {
        if (bodyFrameReader != null)
            bodyFrameReader.Dispose();
        bodyFrameReader = null;

        if (kinectSensor != null)
            kinectSensor.Close();
        kinectSensor = null;
    }

    void InitializeComponent()
    {
        Color[] colors = { Color.black, Color.blue, Color.cyan, Color.gray, Color.green, Color.red };
        for (int i = 0; i < bodyObjects.Length; i++)
            bodyObjects[i] = new BodyObject(colors[i]);

        Color boderColor = Color.yellow;

        left = GameObject.Find("left");
        left.renderer.material.color = boderColor;
        left.SetActive(false);

        right = GameObject.Find("right");
        right.renderer.material.color = boderColor;
        right.SetActive(false);

        top = GameObject.Find("top");
        top.renderer.material.color = boderColor;
        top.SetActive(false);

        bottom = GameObject.Find("bottom");
        bottom.renderer.material.color = boderColor;       
        bottom.SetActive(false);
        
        status = GameObject.Find("status");
        status.guiText.text = kinectSensor.IsAvailable ? "Running" : "No ready Kinect found!";
    }

    void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
    {
        bool dataReceived = false;
        using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
        {
            if (bodyFrame != null)
            {
                if (bodies == null)
                    bodies = new Body[bodyFrame.BodyCount];
                bodyFrame.GetAndRefreshBodyData(bodies);
                dataReceived = true;
            }
        }
        if (dataReceived)
            ProcessBody(bodies);
	
    }

    void Sensor_IsAvailableChanged(object sender, IsAvailableChangedEventArgs e)
    {
        status.guiText.text = kinectSensor.IsAvailable ? "Running" : "Kinect not available!";
    }

	class BodyObject {
		GameObject[] parts;
		GameObject righthand, lefthand;
		bool hide = false;
		public BodyObject(Color color)
		{
			parts = new GameObject[(int)JointType.Count];
			Vector3 scale = new Vector3(0.5f, 0.5f, 0.5f);
			for (int i = 0; i < parts.Length; i++) {
				parts[i] = GameObject.CreatePrimitive(PrimitiveType.Cube);
				parts[i].transform.localScale = scale;
				parts[i].renderer.material.color = color;
				parts[i].transform.position = Vector3.zero;
			}
            Vector3 handScale = new Vector3(3.0f, 3.0f, 3.0f);
			righthand = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            righthand.transform.localScale = handScale;
			lefthand = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            lefthand.transform.localScale = handScale;
			Hide();
		}
		public void SetRightHand(Color color, Vector3 position)
		{
			righthand.SetActive (true);
			righthand.renderer.material.color = color;
			righthand.transform.localPosition = position;
		}
		public void SetLeftHand(Color color, Vector3 position)
		{
			lefthand.SetActive (true);
			lefthand.renderer.material.color = color;
			lefthand.transform.localPosition = position;
		}
		public void HideRightHand()
		{
			righthand.SetActive (false);
		}
		public void HideLeftHand()
		{
			lefthand.SetActive (false);
		}
		public void SetPosition(JointType jointType, Vector3 position)
		{
			hide = false;
			parts [(int)jointType].SetActive (true);
			parts [(int)jointType].transform.localPosition = position;
		}
		public void DrawLines()
		{
			if (hide)
				return;
			GL.Vertex (parts [(int)JointType.Head].transform.position);
			GL.Vertex (parts [(int)JointType.Neck].transform.position);

			GL.Vertex (parts [(int)JointType.SpineShoulder].transform.position);
			GL.Vertex (parts [(int)JointType.Neck].transform.position);
			GL.Vertex (parts [(int)JointType.SpineShoulder].transform.position);

			GL.Vertex (parts [(int)JointType.ShoulderLeft].transform.position);
			GL.Vertex (parts [(int)JointType.ElbowLeft].transform.position);
			GL.Vertex (parts [(int)JointType.WristLeft].transform.position);
			GL.Vertex (parts [(int)JointType.HandLeft].transform.position);
			GL.Vertex (parts [(int)JointType.HandTipLeft].transform.position);
			GL.Vertex (parts [(int)JointType.HandLeft].transform.position);
			GL.Vertex (parts [(int)JointType.ThumbLeft].transform.position);
			GL.Vertex (parts [(int)JointType.HandLeft].transform.position);
			GL.Vertex (parts [(int)JointType.WristLeft].transform.position);
			GL.Vertex (parts [(int)JointType.ElbowLeft].transform.position);
			GL.Vertex (parts [(int)JointType.ShoulderLeft].transform.position);

			GL.Vertex (parts [(int)JointType.SpineShoulder].transform.position);

			GL.Vertex (parts [(int)JointType.ShoulderRight].transform.position);
			GL.Vertex (parts [(int)JointType.ElbowRight].transform.position);
			GL.Vertex (parts [(int)JointType.WristRight].transform.position);
			GL.Vertex (parts [(int)JointType.HandRight].transform.position);
			GL.Vertex (parts [(int)JointType.HandTipRight].transform.position);
			GL.Vertex (parts [(int)JointType.HandRight].transform.position);
			GL.Vertex (parts [(int)JointType.ThumbRight].transform.position);
			GL.Vertex (parts [(int)JointType.HandRight].transform.position);
			GL.Vertex (parts [(int)JointType.WristRight].transform.position);
			GL.Vertex (parts [(int)JointType.ElbowRight].transform.position);
			GL.Vertex (parts [(int)JointType.ShoulderRight].transform.position);

			GL.Vertex (parts [(int)JointType.SpineShoulder].transform.position);

			GL.Vertex (parts [(int)JointType.SpineMid].transform.position);

			GL.Vertex (parts [(int)JointType.SpineBase].transform.position);

			GL.Vertex (parts [(int)JointType.HipLeft].transform.position);
			GL.Vertex (parts [(int)JointType.KneeLeft].transform.position);
			GL.Vertex (parts [(int)JointType.AnkleLeft].transform.position);
			GL.Vertex (parts [(int)JointType.FootLeft].transform.position);
			GL.Vertex (parts [(int)JointType.AnkleLeft].transform.position);
			GL.Vertex (parts [(int)JointType.KneeLeft].transform.position);
			GL.Vertex (parts [(int)JointType.HipLeft].transform.position);

			GL.Vertex (parts [(int)JointType.SpineBase].transform.position);

			GL.Vertex (parts [(int)JointType.HipRight].transform.position);
			GL.Vertex (parts [(int)JointType.KneeRight].transform.position);
			GL.Vertex (parts [(int)JointType.AnkleRight].transform.position);
			GL.Vertex (parts [(int)JointType.FootRight].transform.position);
			GL.Vertex (parts [(int)JointType.AnkleRight].transform.position);
			GL.Vertex (parts [(int)JointType.KneeRight].transform.position);
			GL.Vertex (parts [(int)JointType.HipRight].transform.position);

			GL.Vertex (parts [(int)JointType.SpineBase].transform.position);

			GL.Vertex (parts [(int)JointType.SpineMid].transform.position);

		}
		public void Hide()
		{
			hide = true;
			for (int i = 0; i < parts.Length; i++)
				parts[i].SetActive(false);
			righthand.SetActive (false);
			lefthand.SetActive (false);
		}
	}

	Vector3 AdjustBodyPosition(CameraSpacePoint pos)
	{
		DepthSpacePoint dsp = coordinateMapper.MapCameraPointToDepthSpace (pos);
		return new Vector3 ((dsp.X - Kinect.DEPTH_WIDTH / 2) / 16, (Kinect.DEPTH_HEIGHT - dsp.Y - Kinect.DEPTH_HEIGHT / 2) / 16, 0);
	}

	void ProcessBody(Body[] bodies)
	{
		left.SetActive(false);
		right.SetActive(false);
		top.SetActive(false);
		bottom.SetActive(false);

		for (int i = 0; i < bodies.Length; i++)
		{
			Body body = bodies[i];
			BodyObject bo = bodyObjects[i];
			if (body == null)
				bo.Hide();
			else {
				try {
					if (!body.IsTracked)
						bo.Hide();
					else {
						Vector3[] points = DrawBody(body, bo);

						DrawLeftHand(bo, points[(int)JointType.HandLeft], body.HandLeftState);
						DrawRightHand(bo, points[(int)JointType.HandRight], body.HandRightState);
						DrawClippedEdges(body);

					}
				} catch (Exception e) {
					bo.Hide();
					Debug.Log(string.Format("ProcessBody {0}", e));
				}
			}
		}
	}

	Vector3[] DrawBody(Body body, BodyObject bo)
	{
		IDictionary<JointType, Kinect2.Joint> joints = body.Joints;
		Vector3[] points = new Vector3[joints.Count];
		foreach (JointType jointType in joints.Keys)
		{
			Vector3 p = AdjustBodyPosition(joints[jointType].Position);
			points[(int)jointType] = p;
			if (joints[jointType].TrackingState == TrackingState.Tracked 
			    || joints[jointType].TrackingState == TrackingState.Inferred )
				bo.SetPosition(jointType, p);	
		}
		return points;
	}

	void DrawLeftHand(BodyObject bo, Vector3 handPosition, HandState handState)
	{
		if (HandState.Closed == handState)
			bo.SetLeftHand(Color.red, handPosition);
		else if (HandState.Open == handState)
			bo.SetLeftHand(Color.green, handPosition);
		else if (HandState.Lasso == handState)
			bo.SetLeftHand(Color.blue, handPosition);
		else
			bo.HideLeftHand();
	}
    void DrawRightHand(BodyObject bo, Vector3 handPosition, HandState handState)
    {
        if (HandState.Closed == handState)
            bo.SetRightHand(Color.red, handPosition);
        else if (HandState.Open == handState)
            bo.SetRightHand(Color.green, handPosition);
        else if (HandState.Lasso == handState)
            bo.SetRightHand(Color.blue, handPosition);
        else
            bo.HideRightHand();
    }

	void DrawClippedEdges(Body body)
	{
		FrameEdges clippedEdges = body.ClippedEdges;
		if ((clippedEdges & FrameEdges.Bottom) != 0)
			bottom.SetActive(true);
		if ((clippedEdges & FrameEdges.Top) != 0)
			top.SetActive(true);
		if ((clippedEdges & FrameEdges.Left) != 0)
			left.SetActive(true);
		if ((clippedEdges & FrameEdges.Right) != 0)
			right.SetActive(true);
	}
}
